COMMENT ⊗   VALID 00015 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002		TITLE ARMSOL
C00003 00003	THESE ROUTINES TOGETHER WITH SAITRG[SYS,HE] OCCUPY 1243 OCTAL WORDS
C00004 00004		BEGIN CROSS
C00005 00005		BEGIN SECOND
C00006 00006		BEGIN ANGLE
C00008 00007		BEGIN SOLVE
C00012 00008	WE NOW NEED SOME SINES AND COSINES
C00014 00009	NOW JOINT 4
C00017 00010	DEGEN:	SETZM J+4		JOINT 5 IS ZERO
C00018 00011	PRINC:	0
C00019 00012	IFE WAVE,<
C00022 00013	MUL:	MOVEI J,2
C00023 00014	TH:	BLOCK 6
C00024 00015		BEGIN TRANSMULT
C00025 ENDMK
C⊗;
	TITLE ARMSOL
	↓WAVE←←0	;just assemble the solution for wave
	↓BLUE←←0	;assemble for blue arm
IFN BLUE,<
	↓LH←←1		;left hand arm (blue)
	↓RH←←0		;rignt hand arm (yellow)
>
IFE BLUE,<
	↓LH←←0		;left hand arm (blue)
	↓RH←←1		;rignt hand arm (yellow)
>
;THESE ROUTINES TOGETHER WITH SAITRG[SYS,HE] OCCUPY 1243 OCTAL WORDS

IFN WAVE,<
	INTERNAL ARM.SOLVE
>
IFE WAVE,<
	INTERNAL ANGLE,SOLVE,UPDATE,TIMES
>
	INTERNAL PSOLVE
	EXTERNAL ACOS,SQRT,ATAN2,ASIN,ACOS,SIND,COSD

	↓AC←1
	↓P←17

IFE BLUE,<
	S2V←←6.05
	S6V←←9.38
>
IFN BLUE,<
	S2V←←-6.05
	S6V←←9.425
>
	S22V←←36.6025

↓RAD:	206712273406
IFN BLUE,<
↓SHOLDER:20.035
	51.305
	20.24
>
IFE BLUE,<
↓SHOLDER:20.035
	10.305
	16.24
>
	BEGIN CROSS
;CROSS(R,P,A) RETURNS THE CROSS PRODUCT IN R
;TIME APPROX 180 MICRO SECONDS

	TAC1←←2
	PV1←←3
	PV2←←4
	PV3←←5
	AV1←←6
	AV2←←7
	AV3←←10
	O←←11

↑CROSS:
	HRL AC,-2(P)		;MOVE PV INTO AC'S
	HRRI AC,PV1
	BLT AC,PV3
	HRL AC,-1(P)		;MOVE AV INTO AC'S
	HRRI AC,AV1
	BLT AC,AV3
	MOVE O,-3(P)		
	MOVE AC,PV2
	FMPR AC,AV3
	MOVE TAC1,PV3
	FMPR TAC1,AV2
	FSBR AC,TAC1
	MOVEM AC,(O)
	FMPR PV3,AV1
	FMPR AV3,PV1
	FSBR PV3,AV3
	MOVEM PV3,1(O)
	FMPR PV1,AV2
	FMPR AV1,PV2
	FSBR PV1,AV1
	MOVEM PV1,2(O)
	SUB P,[4(4)]
	JRST @4(P)
	BEND

	BEGIN SECOND
;SECOND(A) RETURNS THE MAGNITUDE SQUARED OF THE VECTOR
;TIME APPROX 90 MICRO SEC

	A←←4

↑SECOND:
	HRL A,-1(P)
	HRRI A,AC
	BLT A,AC+2
	FMPR AC,AC
	FMPR AC+1,AC+1
	FMPR AC+2,AC+2
	FADR AC,AC+1
	FADR AC,AC+2
	SUB P,[2(2)]
	JRST @2(P)
	BEND

	BEGIN ANGLE
;ANGLE(P,A,O) RETURNS THE ANGLE BETWEEN P AND A IN DEGREES ABOUT O
;A AND P MUST BE UNIT VECTORS. I.E. DIRECTION COSINES
;COMPUTES THE ANGLE=ACOS(PV1*AV1 + PV2*AV2 + PV3*AV3)
;AND THEN THE DETERMINANT
;	| O1  O2  O3|
;	|PV1 PV2 PV3|
;	|AV1 AV2 AV3|
;AND IF POSITIVE MULTIPLIES THE ANGLE BY -1;
;TIME APPROX 550 MICRO SECONDS

	TAC1←←2
	PV1←←3
	PV2←←4
	PV3←←5
	AV1←←6
	AV2←←7
	AV3←←10
	O←←11
	I←←13			;12 IS A NO NO

↑ANGLE:
	HRL AC,-3(P)		;MOVE PV INTO AC'S
	HRRI AC,PV1
	BLT AC,PV3
	HRL AC,-2(P)		;MOVE AV INTO AC'S
	HRRI AC,AV1
	BLT AC,AV3
	MOVEI I,2		;COMPUTE DOT PRODUCT
	MOVE AC,PV1
	FMPR AC,AV1
L1:	MOVE TAC1,PV1(I)
	FMPR TAC1,AV1(I)
	FADR AC,TAC1
	SOJG I,L1
	MOVEM AC,DOT
	MOVE O,-1(P)		;COMPUTE DETERMINANT
	MOVE AC,PV2
	FMPR AC,AV3
	MOVE TAC1,PV3
	FMPR TAC1,AV2
	FSBR AC,TAC1
	FMPR AC,(O)
	FMPR PV3,AV1
	FMPR AV3,PV1
	FSBR PV3,AV3
	FMPR PV3,1(O)
	FADR AC,PV3
	FMPR PV1,AV2
	FMPR AV1,PV2
	FSBR PV1,AV1
	FMPR PV1,2(O)
	FADR AC,PV1
	MOVEM AC,DET
	PUSH P,DOT
	PUSHJ P,ACOS
	FMPR AC,RAD
	SKIPL DET		;NEGATE ANGLE IF DET NEGATIVE
	MOVN AC,AC
	SUB P,[4(4)]
	JRST @4(P)
DOT:	0
DET:	0
	BEND

	BEGIN SOLVE
;SOLVE(T,J) GIVEN THE TRANSFORM T, RETURNS THE SOLUTION IN "J"
;PSOLVE(T,J) GIVEN THE TRANSFORM T, RETURNS THE PRINCIPAL SOLUTION IN "J"
;RETURNS FALSE IF THE SOLUTION IS CORRECT
;TIME APPROX 4 M. SEC

	TAC←←2
	I←←3

↑PSOLVE:SETOM PRINC
	SKIPA
IFE WAVE,<
↑SOLVE:
>
IFN WAVE,<
↑ARM.SOLVE:
>
	SETZM PRINC			;DO NOT USE PRINCIPAL SOLUTION
	SETZM FLAG			;SET RETURN FLAG TRUE
	HRL AC,-1(P)			;INITIALIZE J
	HRRI AC,J
	BLT AC,J+5
;COMPUTE W=P-SHOLDER-A*S6
	HRRZ AC,-2(P)		;MOVE P INTO MEMORY
	MOVE TAC,3(AC)
	MOVEM TAC,PV1
	MOVE TAC,7(AC)
	MOVEM TAC,PV1+1
	MOVE TAC,13(AC)
	MOVEM TAC,PV1+2
	MOVE TAC,2(AC)		;MOVE A INTO MEMORY
	MOVEM TAC,AV1
	MOVE TAC,6(AC)
	MOVEM TAC,AV1+1
	MOVE TAC,12(AC)
	MOVEM TAC,AV1+2
	MOVE TAC,1(AC)		;MOVE O INTO MEMORY
	MOVEM TAC,OV1
	MOVE TAC,5(AC)
	MOVEM TAC,OV1+1
	MOVE TAC,11(AC)
	MOVEM TAC,OV1+2
;J2=DOT(W,W)-S2↑2
	MOVN TAC,S22		;INITIALIZE TAC TO -S22
	MOVEI I,2		;COMPUTE THE DOT PRODUCT
L1:	MOVE AC,AV1(I)
	FMPR AC,S6
	FADR AC,SHOLDER(I)
	FSBR AC,PV1(I)
	MOVEM AC,W(I)		;-W(I)
	FMPR AC,AC
	MOVEM AC,W2(I)		;W(I)↑2
	FADR TAC,AC
	SOJGE I,L1
	JUMPLE TAC,[SETZ AC,
		JRST NSR]
	PUSH P,TAC
	PUSHJ P,SQRT
NSR:	CAMLE AC,STOP+3		;CHECK THE STOPS 
	JRST[	MOVE AC,STOP+3
		JRST M3]
	CAMGE AC,STOP+2
	JRST[	MOVE AC,STOP+2
	M3:	SETOM FLAG
		FADR AC,J+2
		FSC AC,-1	;HALF WAY BETWEEN STOP AND LAST JOINT ANGLE
		JRST OK3]
OK3:	MOVEM AC,J+2		;JOINT THREE

;NOW COMPUTE THE BOOM OFFSET
	MOVE AC,W2+1
	FADR AC,W2
	PUSH P,AC
	PUSHJ P,SQRT
	MOVM TAC,S2
	FDVR TAC,AC
	CAML TAC,[1.0]
	MOVSI TAC,(1.0)
	PUSH P,TAC
	PUSHJ P,ASIN
	MOVEM AC,JTEMP

;NOW COMPUTE TH FOR JOINT 1
	PUSH P,W+1
	PUSH P,W
	PUSHJ P,ATAN2
IFN RH,<
	FADR AC,JTEMP		;ADD J
>
IFN LH,<
	FSBR AC,JTEMP
>
	FMPR AC,RAD
	MOVE TAC,AC
IFN RH,<
	FSBR TAC,[90.0]
	JUMPL TAC,ISOK
	FSBR AC,[360.0]
>
IFN LH,<
	FADR TAC,[90.0]
	JUMPG TAC,ISOK
	FADR AC,[360.0]
>
ISOK:	CAMLE AC,STOP+1		;AND CHECK THE STOPS FOR JOINT 1
	JRST[	MOVE AC,STOP+1
		JRST M1]
	CAMGE AC,STOP
	JRST[	MOVE AC,STOP
	M1:	SETOM FLAG
		FADR AC,J
		FSC AC,-1	;HALF WAY BETWEEN STOP AND LAST JOINT ANGLE
		JRST OK1]
OK1:	MOVEM AC,J		;JOINT ONE
;JOINT 2-ACOS(W3/J3)
	MOVN AC,W+2
	FDVR AC,J+2
	MOVEM AC,CO2
	PUSH P,AC
	PUSHJ P,ACOS
	FMPR AC,RAD
	MOVNM AC,J+1		;JOINT TWO
;WE NOW NEED SOME SINES AND COSINES
	PUSH P,J
	PUSHJ P,SIND
	MOVEM AC,SI1
	PUSH P,J
	PUSHJ P,COSD
	MOVEM AC,CO1
	PUSH P,J+1
	PUSHJ P,SIND
	MOVEM AC,SI2
;WE NOW COMPUTE THE UNIT VECTORS AT THE END OF LINK 3
	MOVE AC,CO1
	FMPR AC,CO2
	MOVEM AC,Y
	MOVE AC,SI1
	FMPR AC,CO2
	MOVEM AC,Y+1
	MOVE AC,SI2
	MOVNM AC,Y+2
	FMPR AC,CO1
	MOVEM AC,Z
	MOVE AC,SI1
	FMPR AC,SI2
	MOVEM AC,Z+1
	MOVE AC,CO2
	MOVEM AC,Z+2
;CROSS(VT,Z,A);
	PUSH P,[VT]
	PUSH P,[Z]
	PUSH P,[AV1]
	PUSHJ P,CROSS
;CHECK FOR DEGENERATE CASE
	PUSH P,[VT]
	PUSHJ P,SECOND
	CAMG AC,[0.000001]
	JRST DEGEN		;JOINTS 4 AND 6 ARE DEGENERATE
	PUSH P,AC
	PUSHJ P,SQRT		;GET MAGNITUDE OF VT
	MOVEI I,2
FVT:	MOVE TAC,VT(I)
	FDVR TAC,AC
	MOVEM TAC,VT(I)
	SOJGE I,FVT
VTF:				;VT NOW UNIT MAGNITUDE
;NOW JOINT 4
	PUSH P,[VT]
	PUSH P,[Y]
	PUSH P,[Z]
	PUSHJ P,ANGLE
	MOVE TAC,AC
	FSBR TAC,J+3
	CAMLE TAC,[180.0]
	JRST[	FSBR AC,[360.0]
		JRST TN]
	CAMGE TAC,[-180.0]
	JRST[	FADR AC,[360.0]
		JRST TN]
TN:
	MOVE TAC,AC
	FSBR TAC,J+3
	CAMLE TAC,[90.0]
	JRST[	FSBR AC,[180.0]
		JRST RVTO]
	CAMGE TAC,[-90.0]
	JRST[	FADR AC,[180.0]
	RVTO:	MOVNS VT
		MOVNS VT+1
		MOVNS VT+2
		JRST SOK4]
SOK4:
;NOW CHECK THE STOPS
	CAMLE AC,STOP+5
	JRST[	SKIPN PRINC
		JRST[	MOVE AC,STOP+5
			JRST M4]
		FSBR AC,[180.0]
		JRST RVT]
	CAMGE AC,STOP+4
	JRST[	SKIPN PRINC
		JRST[	MOVE AC,STOP+4
		M4:	SETOM FLAG
			FADR AC,J+3
			FSC AC,-1
			JRST OK4]
		FADR AC,[180.0]
	RVT:	MOVNS VT
		MOVNS VT+1
		MOVNS VT+2
		JRST OK4]
OK4:	MOVEM AC,J+3		;JOINT FOUR
;NOW JOINT 5
	PUSH P,[AV1]
	PUSH P,[Z]
	PUSH P,[VT]
	PUSHJ P,ANGLE
	CAMLE AC,STOP+7
	JRST[	MOVE AC,STOP+7
		JRST M5]
	CAMGE AC,STOP+6
	JRST[	MOVE AC,STOP+6
	M5:	SETOM FLAG
		FADR AC,J+4
		FSC AC,-1	;HALF WAY BETWEEN STOP AND LAST JOINT ANGLE
		JRST OK5]
OK5:	MOVEM AC,J+4
;JOINT 6 IS NOW SIMPLE
	PUSH P,[OV1]
	PUSH P,[VT]
	PUSH P,[AV1]
	PUSHJ P,ANGLE
TOL:	MOVE TAC,AC
	FSBR TAC,J+5
	CAML TAC,[180.0]
	JRST[	FSBR AC,[360.0]
		JRST ST6]
	CAMGE TAC,[-180.0]
	JRST[	FADR AC,[360.0]
		JRST ST6]
ST6:	CAMLE AC,STOP+11
	JRST[	SKIPN PRINC
		JRST[	MOVE AC,STOP+11
			JRST M6]
		FSBR AC,[360.0]
		JRST OK6]
	CAMGE AC,STOP+10
	JRST[	SKIPN PRINC
		JRST[	MOVE AC,STOP+10
		M6:	SETOM FLAG
			FADR AC,J+5
			FSC AC,-1
			JRST OK6]
		FADR AC,[360.0]
		JRST OK6]
OK6:	MOVEM AC,J+5
	HRRZ AC,-1(P)
	HRRZI TAC,(AC)
	HRLI AC,J
	BLT AC,5(TAC)

	MOVE AC,FLAG
	SUB P,[3(3)]
	JRST @3(P)

DEGEN:	SETZM J+4		;JOINT 5 IS ZERO
	PUSH P,[OV1]
	PUSH P,[Y]
	PUSH P,[AV1]
	PUSHJ P,ANGLE
	FSBR AC,J+3		;LEAVE JOINT 4 AS IS
	JRST TOL		;AND PROCEED AS USUAL
PRINC:	0
FLAG:	0
JTEMP:	0
PV1:	BLOCK 3
AV1:	BLOCK 3
OV1:	BLOCK 3
W:	0
SI1:	0
CO1:	0
W2:	0
SI2:	0
CO2:	0
VT:	BLOCK 3
Y:	BLOCK 3
Z:	BLOCK 3
J:	BLOCK 6
S2:	S2V
S22:	S22V
S6:	S6V
TPI:	203622077325
STOP:
IFN BLUE,<
	-90.0
	190.0
	6.2
	34.2
	-325.0
	235.0
	-105.0
	105.0
	-350.0
	220.0
>
IFE BLUE,<
	-185.0
	60.0
	6.5
	27.5
	-175.0
	140.0
	-101.0
	101.0
	-360.0
	360.0
>
	BEND

IFE WAVE,<
	BEGIN UPDATE
;UPDATE(T,J)
;GIVEN JOINT ANGLES J COMPUTES THE TRANSFORMATION T
;TIME APPROX 6 M. SEC

	A1←←2
	A2←←3
	A3←←4
	R1←←5
	R2←←6
	R3←←7
	I←←10
	J←←11
	K←←12

↑UPDATE:
	HRL AC,-1(P)		;TH
	HRRI AC,TH
	BLT AC,TH+5
	SETZM T			;SET T TO UNIT MATRIX
	MOVE AC,[XWD T,T+1]
	BLT AC,T+13
	MOVSI AC,(1.0)
	MOVEM AC,T
	MOVEM AC,T+5
	MOVEM AC,T+12
	SETZM LINK
MATMUL:				;MATMUL	T(K)←T(K-1)*A(K)
	MOVE AC,[XWD T,TN]
	BLT AC,TN+13		;MOVE T INTO TN
	MOVE K,LINK
	CAIN K,2
	JRST[	MOVE AC,SI3
		MOVEM AC,SI
		MOVE AC,CO3
		MOVEM AC,CO
		MOVE AC,TH+2
		MOVEM AC,S+2
		JRST IDSU]
	PUSH P,TH(K)
	PUSHJ P,SIND
	MOVEM AC,SI
	MOVE K,LINK
	PUSH P,TH(K)
	PUSHJ P,COSD
	MOVEM AC,CO
IDSU:	MOVE K,LINK
;FIRST ROW
	MOVE A1,CO
	MOVE A2,SI
	SETZ A3,
	MOVEI I,0
	PUSHJ P,MUL
;SECOND ROW
	MOVN A1,CA(K)
	FMPR A1,SI
	MOVE A2,CA(K)
	FMPR A2,CO
	MOVE A3,SA(K)
	MOVEI I,1
	PUSHJ P,MUL
;THIRD ROW
	MOVE A1,SA(K)
	FMPR A1,SI
	MOVN A2,SA(K)
	FMPR A2,CO
	MOVE A3,CA(K)
	MOVEI I,2
	PUSHJ P,MUL
;FOURTH ROW
	SETZ A1,
	SETZ A2,
	MOVE A3,S(K)
	MOVEI I,3
	PUSHJ P,MUL
	MOVE AC,TN+3		;OLD X
	FADRM AC,T+3		;NEW X
	MOVE AC,TN+7		;OLD Y
	FADRM AC,T+7		;NEW Y
	MOVE AC,TN+13		;OLD Z
	FADRM AC,T+13		;NEW Z
	AOS K,LINK
	CAIGE K,6
	JRST MATMUL
	HRRZ K,-2(P)
	MOVEI AC,13(K)
	HRLI K,T
	BLT K,(AC)
	HRRZ K,-2(P)
	MOVE AC,SHOLDER
	FADRM AC,3(K)		;X
	MOVE AC,SHOLDER+1
	FADRM AC,7(K)		;Y
	MOVE AC,SHOLDER+2
	FADRM AC,13(K)		;Z
	SUB P,[3(3)]
	JRST @3(P)

MUL:	MOVEI J,2
L2:	MOVE AC,PW(J)
	BLT AC,R1+2
	FMPR R1,A1
	FMPR R2,A2
	FMPR R3,A3
	FADR R1,R2
	FADR R1,R3
	MOVEM R1,T(I)
	ADDI I,4
	SOJGE J,L2
	POPJ P,
TH:	BLOCK 6
PW:	XWD TN+10,R1
	XWD TN+4,R1
	XWD TN,R1
LINK:	0
SI:	0
CO:	0
T:	BLOCK 14
TN:	BLOCK 14
SI3:	-1.0
CO3:	0.0
S:	0.0
	S2V
	0.0
	0.0
	0.0
	S6V
SA:	-1.0
	1.0
	0.0
	-1.0
	1.0
	0.0
CA:	0.0
	0.0
	1.0
	0.0
	0.0
	1.0
	BEND
	BEGIN TRANSMULT
	S←1
	M←2
	A1←3
	A2←4
	A3←5
	A4←6
	I←7
	J←10
	R←11
	T←14
	U←13
↑TIMES:HRL T,-1(P)
	HRRI T,MLT
	BLT T,MLT+13
	MOVEI T,MLT
	HRRZ U,-2(P)
	HRRZ R,-3(P)
	MOVEI I,3

L1:	MOVEI J,3

	MOVE A1,(U)
	MOVE A2,1(U)
	MOVE A3,2(U)
L2:	MOVE S,A1
	FMPR S,(T)
	MOVE M,A2
	FMPR M,4(T)
	FADR S,M
	MOVE M,A3
	FMPR M,10(T)
	FADR S,M
	MOVEM S,(R)
	AOJ R,
	AOJ T,
	SOJG J,L2

	FMPR A1,(T)
	FMPR A2,4(T)
	FADR A1,A2
	FMPR A3,10(T)
	FADR A1,A3
	FADR A1,3(U)
	MOVEM A1,(R)
	AOJ R,
	SUBI T,3
	ADDI U,4
	SOJG I,L1
	SUB P,[XWD 4,4]
	JRST @4(P)
MLT:	BLOCK 14
	BEND
>
	END